// RTK includes
#include "rtkConfiguration.h"
#include <rtkFDKBackProjectionImageFilter.h>
#include <rtkConstantImageSource.h>
#include <rtkThreeDCircularProjectionGeometry.h>
#include <rtkRayEllipsoidIntersectionImageFilter.h>
#include <rtkDisplacedDetectorImageFilter.h>
#include <rtkParkerShortScanImageFilter.h>
#include <rtkFDKConeBeamReconstructionFilter.h>
#include <rtkProjectionsReader.h>

// ITK includes
#include <itkImageFileWriter.h>
#include <itkStreamingImageFilter.h>
#include <itkRegularExpressionSeriesFileNames.h>
#include <itkMemoryProbesCollectorBase.h>


//#include "itkVersorRigid3DTransform.h"
#include "itkEuler3DTransform.h"
#include "itkResampleImageFilter.h"
#include "itkFlipImageFilter.h"

using namespace std;

#define round(dTemp) (long(dTemp+(dTemp>0? .5:-.5)))


double vectorMean(const vector<double>& vDouble)
{
	int size = vDouble.size();
	if (size <= 0)
		return 0.0;

	double sum = 0.0;
	vector<double>::const_iterator it;

	for (it = vDouble.begin() ; it != vDouble.end() ; it++)
	{
		sum = sum + (*it);
	}

	return sum / (double)size;	
}

double vectorSum(const vector<double>& vDouble)
{
	int size = vDouble.size();
	if (size <= 0)
		return 0.0;

	double sum = 0.0;
	vector<double>::const_iterator it;

	for (it = vDouble.begin() ; it != vDouble.end() ; it++)
	{
		sum = sum + (*it);
	}

	return sum;
}
//
//double vectorAbsMean(const vector<double>& vDouble)
//{
//	int size = vDouble.size();
//	if (size <= 0)
//		return 0.0;
//
//	double sum = 0.0;
//	vector<double>::const_iterator it;
//
//	for (it = vDouble.begin() ; it != vDouble.end() ; it++)
//	{
//		sum = sum + fabs(*it);
//	}
//
//	return sum / (double)size;	
//}

void GetSelectedIndices(const vector<double>& vFullAngles, vector<double>& vNormAngles, vector<int>& vTargetIdx)
{
	//vector<double>::iterator nomIt;
	
	//vector<double>::const_iterator full_It;//assumption, sorted by projection time. Begins with 179.xxx (CW)
	int latest_Idx = 0;

	double curVal =0.0;
	double nextVal =0.0;

	int sizeNom = vNormAngles.size();
	int sizeFull = vFullAngles.size();
	
	for (int i = 0 ; i< sizeNom ; i++)
	{
		double tmpNominalValue = vNormAngles.at(i);		

		for (int j= latest_Idx+1 ; j<sizeFull-1; j++)
		{	
			//if (j <= latest_Idx)
//				continue;

			curVal = vFullAngles.at(j);
			nextVal = vFullAngles.at(j+1);

			if (curVal > nextVal + 0.2) // e.g.359)  - 0.5,, 0.2-->minimum angle diff.
			{
				if (tmpNominalValue < 100)
					curVal = curVal - 360.0;
				else if (tmpNominalValue > 260)
					nextVal = nextVal + 360.0;					
			}

			if (tmpNominalValue >= curVal && tmpNominalValue <= nextVal)
			{
				double diffCur = fabs(tmpNominalValue - curVal);
				double diffNext = fabs(tmpNominalValue - nextVal);

				latest_Idx = j;

				if (diffCur <= diffNext)
				{
					vTargetIdx.push_back(j);
				}
				else	
				{
					if (j != sizeFull-2)
					{
						vTargetIdx.push_back(j+1);
					}
				}
				break;
			}
		}
	}	
	vTargetIdx.push_back(sizeFull-1);
}

int main(int argc, char *argv[])
{
  // Defines the image type
  typedef float OutputPixelType;
  const unsigned int Dimension = 3;

  typedef itk::Image< OutputPixelType, Dimension > OutputImageType;

  // Generate file names
  char * hisfilesPath = "C:\\Proj654";
  char * geometryXMLPath = "C:\\HIS654\\ElektaGeometry_654.xml";
  char * outputFilePath = "C:\\RTK_proj654_x.mha";

  bool bManualGap = false;
  double gantryAngleInterval = 2.0;

  
  char * regexp = ".*.his";
  std::vector <std::string> names;
  itk::RegularExpressionSeriesFileNames::Pointer regexpnames = itk::RegularExpressionSeriesFileNames::New();
  regexpnames->SetDirectory(hisfilesPath);
  regexpnames->SetNumericSort(false);
  regexpnames->SetRegularExpression(regexp);
  regexpnames->SetSubMatch(0);
  TRY_AND_EXIT_ON_ITK_EXCEPTION( names = regexpnames->GetFileNames() )	
  
cout << names.size() << "  files were found." << endl;  

  // Get the projection geometry
  rtk::ThreeDCircularProjectionGeometryXMLFileReader::Pointer geometryReader;
  geometryReader = rtk::ThreeDCircularProjectionGeometryXMLFileReader::New();
  geometryReader->SetFilename(geometryXMLPath);
  TRY_AND_EXIT_ON_ITK_EXCEPTION( geometryReader->GenerateOutputInformation() )
	  std::cout << "Geometry reading succeed" << std::endl;

  typedef rtk::ThreeDCircularProjectionGeometry GeometryType;
  GeometryType::Pointer fullGeometry = geometryReader->GetOutputObject();
  
  //fullGeometry->GetGantryAngles();

  int geoDataSize = fullGeometry->GetGantryAngles().size();
  cout << "Geometry data size(projection gantry angles): " << geoDataSize << endl;
  if (geoDataSize < 1)
	  return 0;
  //cout << "AngularGapsWithNext Size: " << custGeometry->GetAngularGapsWithNext().size() << endl;
  //cout << "AngularGapsWithNext Mean (deg): " << vectorMean(custGeometry->GetAngularGapsWithNext())/itk::Math::pi * 180.0<< endl;
  cout << "AngularGaps Size: " << fullGeometry->GetAngularGaps().size() << endl;

  double meanGap = vectorMean(fullGeometry->GetAngularGaps())/itk::Math::pi * 180.0;
  cout << "AngularGaps Mean (deg): " << meanGap << endl; 
  double angleSum = vectorSum(fullGeometry->GetAngularGaps())/itk::Math::pi * 180.0;//total rot. range from first angle
  cout << "AngularGaps Sum (deg): " << angleSum  << endl; 


  cout << "Want manual angle gap? (1: Yes, 2: No): ";
  int ans = 0;
  cin >> ans;

  if (ans == 1)
  {
	  bManualGap = true;
	  cout << "Input angle gap in deg: " ;
	  cin >> gantryAngleInterval;

	  if (gantryAngleInterval < meanGap)
	  {
		  cout << "Angle gap size is too small. Terminating the app" << endl;
		  return 0;
		  //bManualGap = false;		  
	  }
  }
  else
	 bManualGap = false;
     

  vector<int> vSelectedIdx;
  vector<int>::iterator itIdx;

  if (bManualGap)
  {
	  //Select indices for recon
	  //Generate norminal gantry values from the first angle
	  double firstAngle = fullGeometry->GetGantryAngles().at(0);
	  double lastAngle = fullGeometry->GetGantryAngles().at(geoDataSize-1);

	  vector<double> vNormAngles;

	  int multiSize  = round(angleSum / gantryAngleInterval)+2;

	  //CW only (179.xx -> 181.xx -> 359.xx --> 1.xx --> 179.xx), CCW should be checked later  
	  for (int i  = 0 ; i< multiSize ; i++)
	  {
		  double curAngle = firstAngle + i*gantryAngleInterval;  

		  if (curAngle >= 360.0)
			  curAngle  = curAngle - 360.0;

		  if (i > multiSize - 5) // last parts of the data
		  {
			  if (curAngle >= lastAngle - gantryAngleInterval)//from 5 latest indices
			  {
				  //gantryAngleInterval/2.0 is given to remove "very near" value to the last value
				  //int aa = 5;
				  //do nothing
			  }
			  else
			  {
				  vNormAngles.push_back(curAngle);
			  }
		  }
		  else
			  vNormAngles.push_back(curAngle);		  
	  }
	  vNormAngles.push_back(lastAngle);

	  //Collect appropriate indices  

	  GetSelectedIndices(fullGeometry->GetGantryAngles(), vNormAngles, vSelectedIdx);
	  
	  for (itIdx =vSelectedIdx.begin() ; itIdx != vSelectedIdx.end() ; itIdx++ )
	  {
		  cout << "Index: " << *itIdx << "     " << "GantryAngle: " << fullGeometry->GetGantryAngles().at(*itIdx) << endl;
	  } 
  }
  else // not manual
  {	  
	  for (int i = 0 ; i< geoDataSize ; i++)
		  vSelectedIdx.push_back(i);
  }
  
  //Regenerate geometry object
  GeometryType::Pointer customGeometry = GeometryType::New();

  for (itIdx =vSelectedIdx.begin() ; itIdx != vSelectedIdx.end() ; itIdx++ )
  {	  
	  //9 parameters are required
	  double curSID = fullGeometry->GetSourceToIsocenterDistances().at(*itIdx);
	  double curSDD = fullGeometry->GetSourceToDetectorDistances().at(*itIdx);
	  double curGantryAngle = fullGeometry->GetGantryAngles().at(*itIdx);

	  double curProjOffsetX = fullGeometry->GetProjectionOffsetsX().at(*itIdx);
	  double curProjOffsetY = fullGeometry->GetProjectionOffsetsY().at(*itIdx);

	  double curOutOfPlaneAngles = fullGeometry->GetOutOfPlaneAngles().at(*itIdx);
	  double curInPlaneAngles = fullGeometry->GetInPlaneAngles().at(*itIdx);

	  double curSrcOffsetX = fullGeometry->GetSourceOffsetsX().at(*itIdx);
	  double curSrcOffsetY = fullGeometry->GetSourceOffsetsY().at(*itIdx);
	  
	  customGeometry->AddProjection(curSID, curSDD,curGantryAngle,
		  curProjOffsetX,curProjOffsetY, //Flexmap 
		  curOutOfPlaneAngles, curInPlaneAngles, //In elekta, these are 0
		  curSrcOffsetX, curSrcOffsetY); //In elekta, these are 0
  }

	  //const std::vector<double> &GetGantryAngles() const {
	  //  return this->m_GantryAngles;
	  //const std::vector<double> &GetSourceToIsocenterDistances() const {
	  //  return this->m_SourceToIsocenterDistances;
	  //const std::vector<double> &GetSourceToDetectorDistances() const {
	  //return this->m_SourceToDetectorDistances;

	  //const std::vector<double> &GetProjectionOffsetsX() const {
	  //return this->m_ProjectionOffsetsX;
	  //}
	  //const std::vector<double> &GetProjectionOffsetsY() const {
	  //return this->m_ProjectionOffsetsY;
	  //}
	  //const std::vector<double> &GetSourceOffsetsX() const {
	  //return this->m_SourceOffsetsX;
	  //}
	  //const std::vector<double> &GetSourceOffsetsY() const {
	  //  return this->m_SourceOffsetsY;
	  //}
	  //const std::vector<double> &GetOutOfPlaneAngles() const {
	  //return this->m_OutOfPlaneAngles;
	  //}
	  //const std::vector<double> &GetInPlaneAngles() const {
	  //  return this->m_InPlaneAngles;
	  //}
  




  //vector<int> vIdxForRecon;
  //first, last scan should be included
    /** Get for each projection the angular gaps with next projection. */
  //const std::vector<double> GetAngularGapsWithNext();
  /** Get for each projection half the angular distance between the previous
   *  and the next projection. */
  //const std::vector<double> GetAngularGaps(); 

  //Regenerate fileNames and geometry object based on the selected indices.

	vector<string> vSelectedFileNames;	
//	vector<int>::iterator itIdx
	for (itIdx = vSelectedIdx.begin() ; itIdx != vSelectedIdx.end() ; itIdx++ )
	{
		string curStr = names.at(*itIdx);
		vSelectedFileNames.push_back(curStr);
	}

	//cout << "Number of selected files: " << vSelectedFileNames.size() << endl;

  // Reads the cone beam projections
  typedef rtk::ProjectionsReader< OutputImageType > ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileNames(vSelectedFileNames);
  TRY_AND_EXIT_ON_ITK_EXCEPTION( reader->GenerateOutputInformation() )
  TRY_AND_EXIT_ON_ITK_EXCEPTION( reader->Update() )
	
  //std::cout << vSelectedFileNames.size() << std::endl;
  std::cout << "Projection reading succeed." << vSelectedFileNames.size() << " files were read" << std::endl;
  
  //// Get the projection geometry
  //rtk::ThreeDCircularProjectionGeometryXMLFileReader::Pointer geometryReader;
  //geometryReader = rtk::ThreeDCircularProjectionGeometryXMLFileReader::New();
  //geometryReader->SetFilename(geometryXMLPath);
  //TRY_AND_EXIT_ON_ITK_EXCEPTION( geometryReader->GenerateOutputInformation() )
  //std::cout << "Geometry reading succeed" << std::endl;

  //typedef rtk::ThreeDCircularProjectionGeometry GeometryType;
  //GeometryType::Pointer custGeometry = geometryReader->GetOutputObject();
  //custGeometry->GetGantryAngles(); 
  

  // Displaced detector weighting // set pipeline //inplace filter
  typedef rtk::DisplacedDetectorImageFilter< OutputImageType > DDFType;
  DDFType::Pointer ddf = DDFType::New();
  ddf->SetInput( reader->GetOutput() );
  //ddf->SetGeometry( geometryReader->GetOutputObject() );
  ddf->SetGeometry( customGeometry );
  cout << "detector weighting success" << endl; 

  // Short scan image filter
  typedef rtk::ParkerShortScanImageFilter< OutputImageType > PSSFType;
  PSSFType::Pointer pssf = PSSFType::New();
  pssf->SetInput( ddf->GetOutput() );
  //pssf->SetGeometry( geometryReader->GetOutputObject() );
  pssf->SetGeometry( customGeometry );
  pssf->InPlaceOff(); //YKComments: Do not overwrite input image buffer for output
  cout << "short scan image filter success" << endl;  
  
  // Generate image sources for cone beam CT reconstruction
  typedef rtk::ConstantImageSource< OutputImageType > ConstantImageSourceType;

  ConstantImageSourceType::PointType origin;
  ConstantImageSourceType::SpacingType spacing;
  ConstantImageSourceType::SizeType sizeOutput;
  
  //--dimension  option: unit: pixels
  //This geometry should be transformed later (in slicer). Ideal geometry is: X(LR):512, Y(AP): 512, Z(SI): 50 --> DICOM STANDARD
  //Prepare empty image buffer
  // Equivalent codes for rtk::SetConstantImageSourceFromGgo<ConstantImageSourceType, args_info_rtkfdk>(constantImageSource, args_info);
  
  sizeOutput[0] = 410; //pixel
  sizeOutput[1] = 120;  
  sizeOutput[2] = 410;

  origin[0] = -0.5*sizeOutput[0]; //Z?
  origin[1] = -0.5*sizeOutput[1];  //Y IEC12106?
  origin[2] = -0.5*sizeOutput[2]; //X?

  spacing[0] = 1.0;
  spacing[1] = 1.0;
  spacing[2] = 1.0;  
  
  ConstantImageSourceType::Pointer constantImageSource = ConstantImageSourceType::New();
  constantImageSource->SetOrigin( origin );
  constantImageSource->SetSpacing( spacing );
  constantImageSource->SetSize( sizeOutput );
  constantImageSource->SetConstant( 0.0 );  
    
 
  // FDK reconstruction filtering
  typedef rtk::FDKConeBeamReconstructionFilter< OutputImageType > FDKCPUType;
  FDKCPUType::Pointer feldkamp = FDKCPUType::New();
  feldkamp->SetInput( 0, constantImageSource->GetOutput() ); // Empty buffer (target)
  feldkamp->SetInput( 1, pssf->GetOutput() ); //projection images (properly filtered)
  //feldkamp->SetGeometry( geometryReader->GetOutputObject() );
  feldkamp->SetGeometry( customGeometry);
  //feldkamp->GetRampFilter()->SetTruncationCorrection(1.0); //rtkfdk default:  0
  //feldkamp->GetRampFilter()->SetHannCutFrequency(5.0);  //rtkfdk default:0 
  //feldkamp->GetRampFilter()->SetHannCutFrequencyY(5.0);  //rtkfdk default: 0

  feldkamp->GetRampFilter()->SetTruncationCorrection(1.0); //rtkfdk default:  0
  feldkamp->GetRampFilter()->SetHannCutFrequency(5.0);  //rtkfdk default:0
  feldkamp->GetRampFilter()->SetHannCutFrequencyY(5.0);  //rtkfdk default:0  
  
  std::cout << "Cone beam reconstruction pipeline is ready" << std::endl;

  // Streaming depending on streaming capability of writer --> not affect the calc. speed
  typedef itk::StreamingImageFilter<OutputImageType, OutputImageType> StreamerType;
  StreamerType::Pointer streamerBP = StreamerType::New();
  streamerBP->SetInput( feldkamp->GetOutput() );
  streamerBP->SetNumberOfStreamDivisions(1); // YK: 1 in example code "rtkfdk" 


  cout << "Euler 3D Transformation: from RTK-procuded volume to standard DICOM coordinate" << endl;

  /* RTK-produced 3D Volume should be changed in coordination of itk */
  /* Coordination transformation using Euler 3D transformation */

  // 1) Prepare Canvas parameter
  //OutputImageType::Pointer fixedImg = OutputImageType::New();	
  //start index: What is the index of Left Top Inferior corner in DICOM coordinate?


  //Same image type from original image -3D & float
  OutputImageType::IndexType start_trans;
  start_trans[0] = 0;
  start_trans[1] = 0;
  start_trans[2] = 0;

  OutputImageType::SizeType size_trans;
  size_trans[0] = sizeOutput[0]; // X //410
  size_trans[1] = sizeOutput[2]; //Y  // 410
  size_trans[2] = sizeOutput[1]; //Z // 120?

  OutputImageType::SpacingType spacing_trans;
  spacing_trans[0] = 1;
  spacing_trans[1] = 1;
  spacing_trans[2] = 1;

  OutputImageType::PointType Origin_trans;
  Origin_trans[0] = size_trans[0]*spacing_trans[0] /-2.0;
  Origin_trans[1] = size_trans[1]*spacing_trans[1] /-2.0;
  Origin_trans[2] = size_trans[2]*spacing_trans[2] /-2.0;	

  OutputImageType::RegionType region_trans;
  region_trans.SetSize(size_trans);
  region_trans.SetIndex(start_trans);
  

  /* 2) Prepare Target image */
  OutputImageType::Pointer targetImg = streamerBP->GetOutput();


  /* 3) Configure transform */  
  typedef itk::Euler3DTransform< double > TransformType;
  TransformType::Pointer transform = TransformType::New();

  TransformType::ParametersType param;
  param.SetSize(6);
  //MAXIMUM PARAM NUMBER: 6!!!
  param.put(0, 0.0); //rot X // 0.5 = PI/2
  param.put(1,  itk::Math::pi/2.0);//rot Y
  param.put(2,  itk::Math::pi/-2.0);//rot Z
  param.put(3, 0.0); // Trans X mm
  param.put(4, 0.0); // Trans Y mm
  param.put(5, 0.0); // Trans Z mm
  
  TransformType::ParametersType fixedParam(3); //rotation center
  fixedParam.put(0,0);
  fixedParam.put(1,0);
  fixedParam.put(2,0);

  transform->SetParameters(param);
  transform->SetFixedParameters(fixedParam); //Center of the Transform

  cout << "Transform matrix:" << "	" << endl;
  cout << transform->GetMatrix() << std::endl;
  
  typedef itk::ResampleImageFilter<OutputImageType, OutputImageType> ResampleFilterType;	
  ResampleFilterType::Pointer resampler = ResampleFilterType::New(); 
  //OutputImageType::RegionType fixedImg_Region = fixedImg->GetLargestPossibleRegion().GetSize();

  resampler->SetInput(targetImg);
  resampler->SetSize(size_trans);
  resampler->SetOutputOrigin( Origin_trans); //Lt Top Inf of Large Canvas
  resampler->SetOutputSpacing( spacing_trans ); // 1 1 1 
  resampler->SetOutputDirection( targetImg->GetDirection() ); //image normal?
  resampler->SetTransform(transform);
 
  //LR flip

  cout << "LR flip filter is being applied" << endl;

  typedef itk::FlipImageFilter< OutputImageType >  FilterType;

  FilterType::Pointer flipFilter = FilterType::New();
  typedef FilterType::FlipAxesArrayType FlipAxesArrayType;

  FlipAxesArrayType arrFlipAxes;
  arrFlipAxes[0] = 1;
  arrFlipAxes[1] = 0;
  arrFlipAxes[2] = 0;

  flipFilter->SetFlipAxes(arrFlipAxes);
  flipFilter->SetInput(resampler->GetOutput());
 
  typedef itk::ImageFileWriter<OutputImageType> WriterType;
  WriterType::Pointer writer = WriterType::New();
  writer->SetFileName(outputFilePath);
  writer->SetUseCompression(true); //not exist in original code (rtkfdk)
  writer->SetInput( flipFilter->GetOutput() );

  cout << "Writing image: " <<outputFilePath << endl;

  try
  {
	  writer->Update();
  }
  catch (itk::ExceptionObject &excp)
  {
	  std::cerr << excp << std::endl;
  }
  std::cout << "Output generation succeed" << std::endl;

   //20131010 Transformation
    
  return 0;
}

